Orientation Control
Your robot must start at the designated line (< 4m from the wall), drive fast forward, and when the robot is within 3ft (914mm = 3 floor tiles in the lab) from the wall, initiate a 180 degree turn.
Here attached is the code controlling the car to turn
case Stunt:
{
// parameter for PID yaw
float kp,ki,kd;
float LastPID = 0.0;
float prevError=0.0,error=0.0,integral=0.0;
float P,I,D,PID;
float current_time=0.0,current_yaw;
bool is_turn = false;
//parameter for extrapolation
float last2_distance = 0.0, last1_distance = 0.0, current_distance=3000.0; //this is used as the initial distance
float last_distance = 3000.0, last_time= (int)millis();
float last1_time = 0.0,last2_time = 0.0;
float curr_speed = 100;
float last_speed;
float yaw_bias;
//get the pid parameter from the computer
success = robot_cmd.get_next_value(kp);
if (!success)
return;
success = robot_cmd.get_next_value(ki);
if (!success)
return;
success = robot_cmd.get_next_value(kd);
if (!success)
return;
success = robot_cmd.get_next_value(yaw_bias);
if (!success)
return;
int start_time = (int)millis();
int turn_time = 0;
int tofcnt = 0;
Serial.println("Start Running! ");
distanceSensor1.startRanging(); //Write configuration bytes to initiate measurement
yaw = 0.0;
//initialize the target_yaw
last_time = millis();
orientation_yaw(); //get the current yaw
float target_yaw = yaw;
bool flag= false;
while ( ((int)millis()-start_time) < 6000 ) //set to collect data for 10s
{
Serial.print(target_yaw);
Serial.print(" ");
Serial.println(current_distance);
if (distanceSensor1.checkForDataReady())
{
//Serial.println("TOF data get!");
// store the data
tof_time[tofcnt] = (float)millis()/1000;
distance1[tofcnt] = distanceSensor1.getDistance(); //Get the result of the measurement from the sensor
distance2[tofcnt] = distanceSensor2.getDistance();
current_distance = distance1[tofcnt];
//update the last1 and last2 distance and last time
last2_distance = last1_distance;
last1_distance = current_distance;
last2_time = last1_time;
last1_time = current_time;
tofcnt++;
distanceSensor1.clearInterrupt();
distanceSensor2.clearInterrupt();
}
else
{
//doing extrapolate
//current distance should be smaller
if(last2_distance!=0&&last2_time!=0) //check if it is just start up
{
tof_time[tofcnt] = (float)millis(); // the unit is ms
current_distance = last1_distance - (last2_distance-last1_distance)*(current_time-last1_time)/(last1_time-last2_time);
distance1[tofcnt] = current_distance;
distance2[tofcnt] = 0;
tofcnt++;
}
// otherwise do not update current_distance
}
if(current_distance<=1500 && !flag )
{
flag = true;
is_turn = true;
turn_time = (int)millis();
target_yaw = target_yaw + yaw_bias;
if(target_yaw>=360)
{
target_yaw -= 360;
}
}
current_time = (int)millis();
orientation_yaw(); //get the current yaw
//Serial.println(yaw);
current_yaw = yaw;
// store the data
// tof_time[tofcnt] = (float)millis()/1000;
curyaw[tofcnt] = yaw;
//update the last1 and last2 distance and last time
if(!is_turn)
{
forward(200);
}
if(is_turn && current_time - turn_time <= 1500)
{
error = current_yaw-target_yaw; //should be larger than 0 at the beginning
P = kp * error;
if((error >= 0 and LastPID <= 0) or (error < 0 and LastPID > 0))
{
integral += error;
integral = min(max(integral,-50),50); //wind-up protection
}
I = ki * integral;
D = kd * (error - prevError);
prevError = error;
PID = P + I + D;
//when yaw decrease, turn left, error < 0
//when yaw increase, turn right, error > 0
PID = min(250,max(PID,-250));
if(PID>=0){turnright((PID/250)*80+70);//curspeed[tofcnt-1] = (PID/250)*200+50;
}
else{turnleft((-PID/250)*80+70);//curspeed[tofcnt-1] = -((-PID/250)*200+50);
}
LastPID = PID;
}
if(is_turn && current_time - turn_time > 1500)
{
forward(200);
}
}
forward(0);
//send the data
//only 929 for 10s count
// Serial.print(tofcnt);
for(int i=0;i< tofcnt;i++)
{
tx_estring_value.clear();
// tx_estring_value.append("time: ");
// tx_estring_value.append(tof_time[i]);
// tx_estring_value.append(" distance 1: ");
// tx_estring_value.append(distance1[i]);
// tx_estring_value.append(" distance 2: ");
// tx_estring_value.append(distance2[i]);
// tx_estring_value.append(" PWM: ");
// tx_estring_value.append(curspeed[i]);
// tx_estring_value.append(" speed: ");
// tx_estring_value.append(instspeed[i]);
// tx_estring_value.append("yaw: ");
tx_estring_value.append(curyaw[i]);
tx_characteristic_string.writeValue(tx_estring_value.c_str());
}
}
break;
The two videos below show the final results:
Here attached is the collected yaw data:
